/*!
* @file Placement.cpp
* @brief 三维定位
* @date 2024/9/12
*/

#include "Placement.h"
#include "Matrix4D.h"

using namespace data;

Placement::Placement(const Vector3d& pos, const Rotation& rot)
    : position(pos), rotation(rot)
{
}

Placement::Placement(const Vector3d& pos, const Rotation& rot, const Vector3d& cnt)
    : rotation(rot)
{
    position = pos + cnt - rot.MultVec(cnt);
}

void Placement::Set(const Vector3d& pos, const Rotation& rot)
{
    position = pos;
    rotation = rot;
}

void Placement::Move(const Vector3d& vector)
{
    position += vector;
}

Placement Placement::operator*(const Placement& p) const
{
    Placement plm(*this);
    plm *= p;
    return plm;
}

Placement& Placement::operator*=(const Placement& p)
{
    return MultRight(p);
}

bool Placement::operator==(const Placement& p) const
{
    return (position == p.position) && (rotation == p.rotation);
}

bool Placement::operator!=(const Placement& p) const
{
    return !(*this == p);
}

Placement& Placement::MultRight(const Placement& p)
{
    position += (p.position * rotation);
    rotation.MultRight(p.rotation);
    return *this;
}

Placement& Placement::MultLeft(const Placement& p)
{
    position *= p;
    rotation.MultLeft(p.rotation);
    return *this;
}

Vector3d Placement::MultVec(const Vector3d& src) const
{
    Vector3d dst = src * rotation;
    dst += position;
    return dst;
}

void Placement::Invert()
{
    rotation.Invert();
    position *= rotation;
    position = -position;
}

Placement Placement::Inverse() const
{
    Placement p(*this);
    p.Invert();
    return p;
}

bool Placement::IsIdentity() const
{
    return (position == Vector3d(0, 0, 0)) && rotation.IsIdentity();
}

bool Placement::IsEqual(const Placement& p, double tol) const
{
    return position.IsEqual(p.position, tol) && rotation.IsEqual(p.rotation, tol);
}

bool Placement::IsIdentity(double tol) const
{
    return IsEqual(Placement(), tol);
}

Matrix4d Placement::ToMatrix() const
{
    Matrix4d matrix = rotation.ToMatrix();
    matrix[3][0] = position.x;
    matrix[3][1] = position.y;
    matrix[3][2] = position.z;
    return matrix;
}
